﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Runtime.InteropServices;
using RayDen.Library.Components.SystemComponents;
using RayDen.Library.Core.Primitives;
using RayDen.Library.Entity.Scene;
using RayDen.RayEngine.Core.Interface;
using RayDen.RayEngine.Core.Types;
using RayDen.RayEngine.Data;

using Triangle=RayDen.Library.Core.Primitives.TriangleDataInfo;

namespace RayDen.RayEngine.IntersectionEngines
{
    public class BoundingVolumeHierarchyAccelerationHelper 
    {
        private const int PrimitivesInNode = 1;
        public int TraversalStackSize = 64;
        public const uint NoHit = RayBuffer.NotHit;

        private int costSamples = 4, isectCost = 80, traversalCost = 10;
        private float emptyBonus = 0.25f;

        public Node RootNode;

     
        public class Node
        {
            public AABB Bound;
            public Node Left, Right;
            public List<Triangle> Primitives;
            public int PrimitiveIndexStart;
        }

        public class BvhBuildNode
        {
            public AABB Bound;
            public int PrimitiveStart;
            public int PrimitiveCount;
        }

        [StructLayout(LayoutKind.Explicit, Size = 32)]
        public struct LNode
        {
            [FieldOffset(0)]
            public AABB Bounds;
            [FieldOffset(24)]
            public UInt32 SkipIndex;
            [FieldOffset(28)]
            public UInt32 PrimitiveIndex;

            //[FieldOffset(28)]
            //public byte Axis;

            public void SetSkipIndex(uint skipIndex)
            {
                this.SkipIndex = skipIndex;
            }

        }
        private Point[] sceneVertices;
        private List<Triangle> triangles;
        private List<ITriangleMesh> meshes;

        public LNode[] nodes;


        private void LinearizeBVH()
        {
            nodes = new LNode[bvhNodesCount];

            this.BuildArray(RootNode, 0);
            //int offset = 0;
            //this.FlattenBVHTree(RootNode, ref offset);
        }

        private int BuildArray(Node node, int offset)
        {
            var nd = node;
            while (nd != null)
            {
                var firstOffset = offset;
                var p = new LNode()
                {
                    Bounds = nd.Bound,
                    PrimitiveIndex = nd.Primitives != null ? (uint) nd.PrimitiveIndexStart : NoHit,
                };
                offset = BuildArray(nd.Left, offset + 1);
                p.SetSkipIndex((uint) offset);
                nodes[firstOffset] = p;
                nd = nd.Right;
            }
            return offset;
        }

        public void BuildTree(List<Triangle> primitives, int start, int end)
        {
            Tracer.TraceLine("Building accelleration {0} triangles", primitives.Count);

            if (start - end == 0)
                return;
            AABB bound = primitives.GetRange(start, end - start).Select(prim => prim.WorldBound(sceneVertices)).Aggregate((b1, b2) => b1.Union(b2));
            RootNode = new BoundingVolumeHierarchyAccelerationHelper.Node() { Bound = bound };
            bvhNodesCount++;
            if ((end - start) <= PrimitivesInNode)
            {
                RootNode.Primitives = primitives.GetRange(start, (end - start));
                RootNode.PrimitiveIndexStart = start;
                return;
            }



            RootNode.Left = this.BuildSubTree(primitives, start, (start + end) / 2);
            RootNode.Right = this.BuildSubTree(primitives, (start + end) / 2 + 1, end);

            Tracer.TraceLine("Linearize BVH");
            this.LinearizeBVH();
            Tracer.TraceLine("Complete");

        }


        private Node BuildQualityTree(List<BvhBuildNode> primitives, int start, int end, AABB parentBounds)
        {
            if (end-start < 0)
                return null;
            var subPrimitives = primitives.GetRange(start, end - start);
            var node = new BoundingVolumeHierarchyAccelerationHelper.Node()  { Bound = parentBounds };
            bvhNodesCount++;
            if ((end - start) <= PrimitivesInNode)
            {
                node.Primitives = triangles.GetRange(primitives[start].PrimitiveStart, primitives[start].PrimitiveCount);                    
                node.PrimitiveIndexStart = start;
                return node;
            }
            if (end - start == 2)
            {
                bvhNodesCount+=3;
                node.Left =
                    new Node()
                    {
                        Primitives = triangles.GetRange(primitives[start].PrimitiveStart, primitives[start].PrimitiveCount),
                        PrimitiveIndexStart = start,
                        Bound = parentBounds
                    };
                node.Right = new Node()
                {
                    Left = CreateLeafNode(primitives, start + 1, end, parentBounds),
                    Primitives = triangles.GetRange(primitives[end].PrimitiveStart, primitives[end].PrimitiveCount),
                    PrimitiveIndexStart = end,
                    Bound = parentBounds
                };

                return node;
            }
            float splitVal;
            int axit;

            FindBestSplit(primitives, start, end, out splitVal, out axit);
            var v = new Vector();
            v[axit] = splitVal;
            var splitPoint = parentBounds.Center;
            splitPoint[axit] = splitVal;
            subPrimitives.Sort((a, b) => ComparePrimitives(a, b, (Axis) axit));

            var lb = new AABB(parentBounds.Min, splitPoint);
            var rb = new AABB(splitPoint, parentBounds.Max);

            int md = 0;
            /*
             * , lbRl = 0
            for (int i = 0; i < subPrimitives.Count; i++)
            {

                if (lb.Overlaps(subPrimitives[i].Bound))
                {
                    md++;
                    lbRl++;
                }
                
                if (rb.Overlaps(subPrimitives[i].WorldBounds))
                {
                    md--;
                    rbRL++;
                }
            }*/
            node.Bound = lb;

            var mid = start + md;

            if ((end - start) <= (start + mid))
            {
                if (md == 0)
                {
                    mid = start + (end - start)/2;
                }
            }
  

            //(start + end) / 2;
            node.Left = this.BuildQualityTree(primitives, start, mid, lb);
            node.Right = this.BuildQualityTree(primitives, mid + 1, end, rb);
            return node;
        }

        private Node CreateLeafNode(List<BvhBuildNode> primitives, int start, int end, AABB parentBounds, Node left = null, Node right = null)
        {
            return new Node()
                {
                    Bound = parentBounds,
                    PrimitiveIndexStart = start,
                    Primitives = triangles.GetRange(primitives[end].PrimitiveStart, primitives[end].PrimitiveCount),
                    Left = left,
                    Right = right
                };
        }

        private Node BuildSubTree(List<Triangle> primitives, int start, int end)
        {
            if (start - end == 0)
                return null;
            AABB bound = primitives.GetRange(start, end - start).Select(prim => prim.WorldBound(sceneVertices)).Aggregate((b1, b2) => b1.Union(b2));
            var node = new Node() { Bound = bound };
            bvhNodesCount++;
            if ((end - start) == PrimitivesInNode)
            {
                node.Primitives = primitives.GetRange(start, (end - start));
                node.PrimitiveIndexStart = start;
                return node;
            }
            var mid = (start + end) / 2;
            node.Left = this.BuildSubTree(primitives, start, mid);
            node.Right = this.BuildSubTree(primitives, mid+1, end);
            return node;
        }



        #region IAccellerationStructure Members

        private int bvhNodesCount;
        public void Init(RayEngineScene scene, ITriangleMesh[] geometry)
        {
            var sceneModel = scene;
            meshes = new List<ITriangleMesh>();
            meshes.AddRange(geometry);
            this.sceneVertices = sceneModel.Vertices;
            var trianglesCache = scene.Triangles.ToList();
            Comparison<Triangle> tc = (t1, t2) => CompareTriangles(t1, t2);
            //trianglesCache.Sort(tc);
            this.triangles = trianglesCache;
            //this.BuildTree(trianglesCache, 0, trianglesCache.Count);
            AABB bound = trianglesCache.Select(prim => prim.WorldBound(sceneVertices)).Aggregate((b1, b2) => b1.Union(b2));
            var prims =
                trianglesCache.Select(
                    (item, index) =>
                    new BvhBuildNode()
                        {
                            Bound = item.WorldBound(sceneVertices),
                            PrimitiveCount = 1,
                            PrimitiveStart = index
                        }).ToList();
            this.RootNode = BuildQualityTree(prims, 0, trianglesCache.Count, bound);

            Tracer.TraceLine("Linearize BVH");
            this.LinearizeBVH();
            Tracer.TraceLine("Complete");

            Tracer.TraceLine("Tree is builded, {0} nodes total", bvhNodesCount);
        }

        public int CompareTriangles(Triangle t1, Triangle t2, Axis a = Axis.PLANE_Y)
        {
            var p1 = t1.Center(sceneVertices);
            var p2 = t2.Center(sceneVertices);
            return p1.Component(a).CompareTo(p2.Component(a));
        }

        public int ComparePrimitives(BvhBuildNode t1, BvhBuildNode t2, Axis a = Axis.PLANE_Y)
        {
            var p1 = t1.Bound.Center;
            var p2 = t2.Bound.Center;
            return p1.Component(a).CompareTo(p2.Component(a));
        }
        #endregion
        /*
        public bool Intersect(RayInfo ray)
        {
#if LBVH
            return BVHAccel_SIntersect(ray);

            //return this.ShadowIntersectBVH(ray);
#else
            return IntersectNode(RootNode, ray);
#endif
        }



        public bool Intersect(RayInfo ray, out IntersectionInfo intersection)
        {
#if LBVH
            intersection = new IntersectionInfo() { Distance = 1000000f };
            return BVHAccel_Intersect(ray, ref intersection);
            //return IntersectBVH(ray, ref intersection);
#else
            return IntersectNode(RootNode, ray, out intersection);
#endif
        }

        private bool IntersectNode(BoundingVolumeHierarchyAcceleration.Node node, RayInfo ray, out IntersectionInfo intersection)
        {
            if (node == null)
            {
                intersection = null;
                return false;
            }
            float t0, t1 = 0f, dist = float.MaxValue, u = 0f, v = 0f;
            if (BVHAccellerationStructure.IntersectBox(node.Bound, ray))
            {
                //ray.MinT = t0;
                //ray.MaxT = t1;

                if (node.Primitives != null && node.Primitives.Count > 0)
                {
                    for (int i = 0; i < node.Primitives.Count; i++)
                    {
                        if (node.Primitives[i].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v) && dist > t1)
                        {
                            var triangleMesh = node.Primitives[i].Owner;
                            intersection = new IntersectionInfo();
                            dist = t1;
                            intersection.Distance = dist;
                            intersection.Geometry = node.Primitives[i];
                            intersection.Point = ray.Point(dist);
                            intersection.Material = materialProvider.Get(triangleMesh.MaterialName);

                            var gnormal =
                                intersection.Geometry.InterpolateMeshNormal(u, v).Normalize();
                            //InterpolateNormal(ref sceneVertices, u, v).Normalize();
                            //triangleMesh.GetTriangleNormal(i, 0);
                            //triangleMesh.InterpolateTriangleNormal(i, u, v).Normalize();
                            Normal shadeN;

                            if (Vector.Dot(ref ray.Dir, ref gnormal) >= 0f)
                                shadeN = -gnormal;
                            else
                                shadeN = gnormal;

                            intersection.ShadingNormal = shadeN;
                            intersection.Normal = gnormal;
                            //                                    triangleMesh.InterpolateTriangleNormal(i, u, v);
                            //triangleMesh.triangles[i].InterpolateNormal(ref scene.Vertices, u, v).Normalize();
                            //triangleMesh.triangles[i].InterpolateNormal(ref scene.Vertices, u, v).Normalize();
                            //triangleMesh.triangles[i].ComputeNormal(ref scene.Vertices).Normalize();

                            intersection.TexCoords = new UV(u, v);
                            //triangleMesh.InterpolateTriUV(i, u, v);
                            return true;
                        }
                    }
                }

                IntersectionInfo li, ri;
                var leftIntersect = IntersectNode(node.Left, ray, out li);
                var rIntersect = IntersectNode(node.Right, ray, out ri);
                if (leftIntersect & rIntersect)
                {
                    intersection = li.Distance > ri.Distance ? ri : li;
                    return true;
                }
                if (leftIntersect)
                {
                    intersection = li;
                    return true;
                }
                if (rIntersect)
                {
                    intersection = ri;
                    return true;
                }

            }
            intersection = null;
            return false;
        }


        private bool IntersectNode(BoundingVolumeHierarchyAcceleration.Node node, RayInfo ray)
        {
            if (node == null)
            {
                return false;
            }
            float t1 = 0f, u = 0f, v = 0f;
            if (BVHAccellerationStructure.IntersectBox(node.Bound, ray))
            {
                if (node.Primitives != null && node.Primitives.Count > 0)
                {
                    for (int i = 0; i < node.Primitives.Count; i++)
                    {
                        if (node.Primitives[i].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v))
                        {
                            return true;
                        }
                    }
                }
                return IntersectNode(node.Left, ray) || IntersectNode(node.Right, ray);
            }
            return false;
        }

        #endregion




        #region Linear BVH

        private bool IntersectBVH(RayInfo ray, ref IntersectionInfo sp)
        {
            if (sp == null)
            {
                sp = new IntersectionInfo();
            }
            if (nodes.Length == 0)
                return false;
            TriangleMesh lastHitMesh = null;
            int lastHitIndex = -1, triangleIndex = -1;
            float u = 0f, v = 0f;
            bool hit = false;
            Vector invDir = ray.InvDir;
            //new Vector(1f / ray.Dir.x, 1f / ray.Dir.y, 1f / ray.Dir.z);
            var dirIsNeg = new[] { invDir.x < 0, invDir.y < 0, invDir.z < 0 };
            int todoOffset = 0, nodeNum = 0;
            int[] todo = new int[TraversalStackSize];
            var dist = float.MaxValue;
            while (true)
            {
                float t1 = 0f;
                if (BVHAccellerationStructure.IntersectBox(nodes[nodeNum].Bounds, ray))
                {
                    //if (node.bbox.Intersect(vray, out t0, out t2)) {
                    if (nodes[nodeNum].PrimitiveIndex != NoHit)
                    {
                        if (triangles[nodes[nodeNum].PrimitiveIndex].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v))
                        {
                            if (dist > t1)
                            {
                                triangleIndex = nodes[nodeNum].PrimitiveIndex;
                                lastHitMesh = triangles[nodes[nodeNum].PrimitiveIndex].Owner;
                                lastHitIndex = triangles[nodes[nodeNum].PrimitiveIndex].Index;
                                dist = t1;
                                sp.Point = ray.Point(dist);
                                sp.Geometry = triangles[nodes[nodeNum].PrimitiveIndex];
                                sp.Distance = dist;
                                hit = true;
                            }
                        }
                        if (todoOffset == 0)
                            break;
                        nodeNum = todo[--todoOffset];
                    }
                    else
                    {
                        if (dirIsNeg[nodes[nodeNum].Bounds.MaximumExtent()])
                        {
                            todo[todoOffset++] = nodeNum + 1;
                            nodeNum = nodes[nodeNum].SkipIndex;
                        }
                        else
                        {
                            todo[todoOffset++] = nodes[nodeNum].SkipIndex;
                            nodeNum = nodeNum + 1;
                        }
                    }
                }
                else
                {
                    if (todoOffset == 0)
                        break;
                    nodeNum = todo[--todoOffset];
                }
            }

            if (hit)
            {
                sp.Material = materialProvider.Get(lastHitMesh.MaterialName);
                var gnormal = triangles[triangleIndex].InterpolateMeshNormal(u, v).Normalize();
                Normal shadeN;

                if (Vector.Dot(ref ray.Dir, ref gnormal) >= 0f)
                    shadeN = -gnormal;
                else
                    shadeN = gnormal;

                sp.ShadingNormal = shadeN;
                sp.Normal = gnormal;
                //triangles[node.primOffset + i].InterpolateNormal(ref sceneModel.Vertices, u, v).Normalize();
                sp.TexCoords = lastHitMesh.InterpolateTriUV(lastHitIndex, u, v);
                return true;
            }

            return false;
        }

        private bool ShadowIntersectBVH(RayInfo ray)
        {
            if (nodes.Length == 0)
                return false;
            bool hit = false;
            Vector invDir = ray.InvDir;
            //new Vector(1f / ray.Dir.x, 1f / ray.Dir.y, 1f / ray.Dir.z);
            var dirIsNeg = new[] { invDir.x < 0, invDir.y < 0, invDir.z < 0 };
            int todoOffset = 0, nodeNum = 0;
            int[] todo = new int[TraversalStackSize];
            while (true)
            {
                float t1 = 0f, u = 0f, v = 0f;
                if (BVHAccellerationStructure.IntersectBox(nodes[nodeNum].Bounds, ray))
                {
                    if (nodes[nodeNum].PrimitiveIndex != NoHit)
                    {
                        if (triangles[nodes[nodeNum].PrimitiveIndex].Intersect(ref sceneVertices, ref ray, ref t1, ref u,
                                                                               ref v))
                        {
                            hit = true;
                            break;
                        }

                        if (todoOffset == 0)
                            break;
                        nodeNum = todo[--todoOffset];
                    }
                    else
                    {
                        if (dirIsNeg[nodes[nodeNum].Bounds.MaximumExtent()])
                        {
                            todo[todoOffset++] = nodeNum + 1;
                            nodeNum = nodes[nodeNum].SkipIndex;
                        }
                        else
                        {
                            todo[todoOffset++] = nodes[nodeNum].SkipIndex;
                            nodeNum = nodeNum + 1;
                        }
                    }
                }
                else
                {
                    if (todoOffset == 0)
                        break;
                    nodeNum = todo[--todoOffset];
                }
            }

            return hit;
        }


        private bool BVHAccel_Intersect(RayInfo ray, ref IntersectionInfo sp)
        {
            float u = 0f, v = 0f, t1 = 0f;
            int currentNode = 0; // Root Node
            int stopNode = nodes.Length; // Non-existent
            bool hit = false;
            TriangleMesh lastHitMesh = null;
            int lastHitIndex = -1, triangleIndex = -1;
            while (currentNode < stopNode)
            {
                if (BVHAccellerationStructure.IntersectBox(nodes[currentNode].Bounds, ray))
                {
                    if (nodes[currentNode].PrimitiveIndex != NoHit)
                    {
                        if (triangles[nodes[currentNode].PrimitiveIndex].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v))
                        {
                            if (t1 < sp.Distance)
                            {
                                hit = true; // Continue testing for closer intersections

                                sp.Point = ray.Point(t1);
                                sp.Geometry = triangles[nodes[currentNode].PrimitiveIndex];
                                sp.Distance = t1;
                                triangleIndex = nodes[currentNode].PrimitiveIndex;
                                lastHitMesh = triangles[nodes[currentNode].PrimitiveIndex].Owner;
                                lastHitIndex = triangles[nodes[currentNode].PrimitiveIndex].Index;
                            }
                        }
                    }
                    currentNode++;
                }
                else
                    currentNode = nodes[currentNode].SkipIndex;
            }

            if (hit)
            {
                sp.Material = materialProvider.Get(lastHitMesh.MaterialName);
                var gnormal = triangles[triangleIndex].InterpolateMeshNormal(u, v).Normalize();
                Normal shadeN;

                if (Vector.Dot(ref ray.Dir, ref gnormal) >= 0f)
                    shadeN = -gnormal;
                else
                    shadeN = gnormal;

                sp.ShadingNormal = shadeN;
                sp.Normal = gnormal;
                //triangles[node.primOffset + i].InterpolateNormal(ref sceneModel.Vertices, u, v).Normalize();
                sp.TexCoords = lastHitMesh.InterpolateTriUV(lastHitIndex, u, v);
                return true;
            }


            return hit;
        }
        private bool BVHAccel_SIntersect(RayInfo ray)
        {
            float u = 0f, v = 0f, t1 = 0f;
            int currentNode = 0; // Root Node
            int stopNode = nodes.Length; // Non-existent
            while (currentNode < stopNode)
            {
                if (BVHAccellerationStructure.IntersectBox(nodes[currentNode].Bounds, ray))
                {
                    if (nodes[currentNode].PrimitiveIndex != NoHit)
                    {
                        if (triangles[nodes[currentNode].PrimitiveIndex].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v))
                        {
                            return true;
                        }
                    }
                    currentNode++;
                }
                else
                    currentNode = nodes[currentNode].SkipIndex;
            }
            return false;

        }

        #endregion
        */
        void FindBestSplit(List<Triangle> list, int begin, int end, out float splitValue, out int bestAxis)
        {
            splitValue = 0;
            if (end - begin == 2)
            {
                splitValue = (list[begin].WorldBound(sceneVertices).Max[0] + list[begin].WorldBound(sceneVertices).Min[0] + list[end - 1].WorldBound(sceneVertices).Max[0] + list[end - 1].WorldBound(sceneVertices).Min[0]) / 2f;
                bestAxis = 0;
            }
            else
            {
                Vector mean2 = new Vector(), var = new Vector();

                for (int i = begin; i < end; i++)
                    mean2 += (Vector) (list[i].WorldBound(sceneVertices).Max + list[i].WorldBound(sceneVertices).Min);
                mean2 /= end - begin;

                for (int i = begin; i < end; i++)
                {
                    Vector v = list[i].WorldBound(sceneVertices).Max + list[i].WorldBound(sceneVertices).Min - mean2;
                    v.x *= v.x;
                    v.y *= v.y;
                    v.z *= v.z;
                    var += v;
                }
                // Select axis with more variance
                if (var.x > var.y && var.x > var.z)
                    bestAxis = 0;
                else if (var.y > var.z)
                    bestAxis = 1;
                else
                    bestAxis = 2;

                if (costSamples > 1)
                {
                    AABB nodeBounds = new AABB();//!!!!
                    for (int i = begin; i < end; i++)
                        nodeBounds = nodeBounds.Union(list[i].WorldBound(sceneVertices));

                    Vector d = nodeBounds.Max - nodeBounds.Min;
                    float totalSA = (2f * (d.x * d.y + d.y * d.z + d.z * d.x));
                    float invTotalSA = 1f / totalSA;

                    // Sample cost for split at some points
                    float increment = 2f * d[bestAxis] / (costSamples + 1);
                    float bestCost = float.PositiveInfinity;
                    for (float splitVal = 2f * nodeBounds.Min[bestAxis] + increment; splitVal < 2f * nodeBounds.Max[bestAxis]; splitVal += increment)
                    {
                        int nBelow = 0, nAbove = 0;
                        AABB bbBelow = new AABB(Point.Zero), bbAbove = new AABB(Point.Zero);
                        for (int j = begin; j < end; j++)
                        {
                            if ((list[j].WorldBound(sceneVertices).Max[bestAxis] + list[j].WorldBound(sceneVertices).Min[bestAxis]) < splitVal)
                            {
                                nBelow++;
                                bbBelow = bbBelow.Union(list[j].WorldBound(sceneVertices));
                            }
                            else
                            {
                                nAbove++;
                                bbAbove = bbAbove.Union(list[j].WorldBound(sceneVertices));
                            }
                        }
                        Vector dBelow = bbBelow.Max - bbBelow.Min;
                        Vector dAbove = bbAbove.Max - bbAbove.Min;
                        float belowSA = 2f * ((dBelow.x * dBelow.y + dBelow.x * dBelow.z + dBelow.y * dBelow.z));
                        float aboveSA = 2f * ((dAbove.x * dAbove.y + dAbove.x * dAbove.z + dAbove.y * dAbove.z));
                        float pBelow = belowSA * invTotalSA;
                        float pAbove = aboveSA * invTotalSA;
                        float eb = (nAbove == 0 || nBelow == 0) ? emptyBonus : 0;
                        float cost = traversalCost + isectCost * (1f - eb) * (pBelow * nBelow + pAbove * nAbove);
                        // Update best split if this is lowest cost so far
                        if (cost < bestCost)
                        {
                            bestCost = cost;
                            splitValue = splitVal;
                        }
                    }
                }
                else
                {
                    // Split in half around the mean center
                    splitValue = mean2[bestAxis];
                }
            }
        }


        void FindBestSplit(List<BvhBuildNode> list, int begin, int end, out float splitValue, out int bestAxis)
        {
            splitValue = 0;
            if (end - begin == 2)
            {
                splitValue = (list[begin].Bound.Max[0] + list[begin].Bound.Min[0] + list[end - 1].Bound.Max[0] + list[end - 1].Bound.Min[0]) / 2f;
                bestAxis = 0;
            }
            else
            {
                Vector mean2 = new Vector(), var = new Vector();

                for (int i = begin; i < end; i++)
                    mean2 += (Vector)(list[i].Bound.Max + list[i].Bound.Min);
                mean2 /= end - begin;

                for (int i = begin; i < end; i++)
                {
                    Vector v = list[i].Bound.Max + list[i].Bound.Min - mean2;
                    v.x *= v.x;
                    v.y *= v.y;
                    v.z *= v.z;
                    var += v;
                }
                // Select axis with more variance
                if (var.x > var.y && var.x > var.z)
                    bestAxis = 0;
                else if (var.y > var.z)
                    bestAxis = 1;
                else
                    bestAxis = 2;

                if (costSamples > 1)
                {
                    AABB nodeBounds = new AABB();//!!!!
                    for (int i = begin; i < end; i++)
                        nodeBounds = nodeBounds.Union(list[i].Bound);

                    Vector d = nodeBounds.Max - nodeBounds.Min;
                    float totalSA = (2f * (d.x * d.y + d.y * d.z + d.z * d.x));
                    float invTotalSA = 1f / totalSA;

                    // Sample cost for split at some points
                    float increment = 2f * d[bestAxis] / (costSamples + 1);
                    float bestCost = float.PositiveInfinity;
                    for (float splitVal = 2f * nodeBounds.Min[bestAxis] + increment; splitVal < 2f * nodeBounds.Max[bestAxis]; splitVal += increment)
                    {
                        int nBelow = 0, nAbove = 0;
                        AABB bbBelow = new AABB(Point.Zero), bbAbove = new AABB(Point.Zero);
                        for (int j = begin; j < end; j++)
                        {
                            if ((list[j].Bound.Max[bestAxis] + list[j].Bound.Min[bestAxis]) < splitVal)
                            {
                                nBelow++;
                                bbBelow = bbBelow.Union(list[j].Bound);
                            }
                            else
                            {
                                nAbove++;
                                bbAbove = bbAbove.Union(list[j].Bound);
                            }
                        }
                        Vector dBelow = bbBelow.Max - bbBelow.Min;
                        Vector dAbove = bbAbove.Max - bbAbove.Min;
                        float belowSA = 2f * ((dBelow.x * dBelow.y + dBelow.x * dBelow.z + dBelow.y * dBelow.z));
                        float aboveSA = 2f * ((dAbove.x * dAbove.y + dAbove.x * dAbove.z + dAbove.y * dAbove.z));
                        float pBelow = belowSA * invTotalSA;
                        float pAbove = aboveSA * invTotalSA;
                        float eb = (nAbove == 0 || nBelow == 0) ? emptyBonus : 0;
                        float cost = traversalCost + isectCost * (1f - eb) * (pBelow * nBelow + pAbove * nAbove);
                        // Update best split if this is lowest cost so far
                        if (cost < bestCost)
                        {
                            bestCost = cost;
                            splitValue = splitVal;
                        }
                    }
                }
                else
                {
                    // Split in half around the mean center
                    splitValue = mean2[bestAxis];
                }
            }
        }

    }
}
